#include "ping.h"
#include <stdio.h>
#include "socket.h"
#include "wizchip_conf.h"
#include "w5500.h"
#include "pico/stdlib.h"

#define PING_DEBUG
PINGMSGR PingRequest;	 
PINGMSGR PingReply;	 
#define Sn_PROTO(ch)                (0x001408 + (ch<<5))  

static uint16_t RandomID = 0x1234; 
static uint16_t RandomSeqNum = 0x4321;
uint8_t ping_reply_received = 0; 
uint8_t req=0;
uint8_t rep=0;

unsigned short checksum(
	unsigned char * src, 	/**< pointer to stream  */
	unsigned int len		/**< size of stream */
	)
{
	u_int sum, tsum, i, j;
	u_long lsum;

	j = len >> 1;

	lsum = 0;

	for (i = 0; i < j; i++) 
	{
		tsum = src[i * 2];
		tsum = tsum << 8;
		tsum += src[i * 2 + 1];
		lsum += tsum;
	}

	if (len % 2) 
	{
		tsum = src[i * 2];
		lsum += (tsum << 8);
	}

	sum = lsum;
	sum = ~(sum + (lsum >> 16));
	return (u_short) sum;	
}

uint16_t swaps(uint16_t i)
{
  uint16_t ret=0;
  ret = (i & 0xFF) << 8;
  ret |= ((i >> 8)& 0xFF);
  return ret;	
}
uint32_t swapl(uint32_t l)
{
  uint32_t ret=0;
  ret = (l & 0xFF) << 24;
  ret |= ((l >> 8) & 0xFF) << 16;
  ret |= ((l >> 16) & 0xFF) << 8;
  ret |= ((l >> 24) & 0xFF);
  return ret;
}

uint16_t htons(uint16_t hostshort)	/**< A 16-bit number in host byte order.  */
{
	#if ( SYSTEM_ENDIAN == _ENDIAN_LITTLE_ )
		return swaps(hostshort);
	#else
		return hostshort;
	#endif		
}

void ping_auto(uint8_t s, uint8_t *addr)
{
	uint8_t i;
	int32_t len = 0;
	uint8_t cnt=0;
	for(i = 0; i<=3;i++)																		
	{
		sleep_ms(10); 
		switch(getSn_SR(s))																		
		{
			case SOCK_CLOSED:																		
				close(s);
				IINCHIP_WRITE(Sn_PROTO(s), IPPROTO_ICMP);           
				if(socket(s,Sn_MR_IPRAW,3000,0)!=0)									
				{ }
				while(getSn_SR(s)!=SOCK_IPRAW);
				sleep_ms(2000); 																																				
			break;
				
			case SOCK_IPRAW:																		
				ping_request(s, addr);															
				req++;
				sleep_ms(50);
				while(1)
				{	
					if ( (len = getSn_RX_RSR(s) ) > 0)
					{	
						ping_reply(s, addr, len);												
						sleep_ms(50); 																	
						rep++;
						break;
					}
					else if(cnt > 200)
					{
						printf( "Request Time out. \r\n");
						cnt = 0;
						break;
					}
					else
					{
						cnt++;
						sleep_ms(50); 																	/*wait 50ms*/
					}
					// wait_time for 2 seconds, Break on fail
				}
	
			break;
			default:
			break;
		}
			#ifdef PING_DEBUG
		if(rep!=0)
		{
			printf("Ping Request = %d, PING_Reply = %d\r\n",req,rep);
			if(rep == req)
				printf( "PING SUCCESS\r\n " );
			else
				printf( "REPLY_ERROR\r\n " );
		}	
    else{}		
			#endif  
	}

}

void ping_count(uint8_t s, uint16_t pCount, uint8_t *addr)
{
	uint16_t rlen, cnt,i;
	cnt = 0;
	
	for(i=0; i<pCount+1;i++)																	
	{

		if(i!=0)
		{
			/* Output count number */
			printf( "\r\nNo.%d\r\n",   (i-1));
		}
		switch(getSn_SR(s))	
		{																		
			case SOCK_CLOSED:																			
				close(s); 
				/* Create Socket */  
				IINCHIP_WRITE(Sn_PROTO(s), IPPROTO_ICMP);						
				/* Check socket register */
				while(getSn_SR(s)!=SOCK_IPRAW);		
				sleep_ms(1000); 																		
				sleep_ms(1000); 																		
			break;
			
			case SOCK_IPRAW:																			
				ping_request(s, addr);															
				req++;			
				while(1)
				{
					if ( (rlen = getSn_RX_RSR(s) ) > 0)
					{
						ping_reply(s, addr, rlen);											
						rep++;
						if (ping_reply_received)  break;				
					}		
					if ( (cnt > 100) ) 
					{
						printf( "\r\nRequest Time out. \r\n") ;
						cnt = 0;
						break;
					}
					else 
					{ 
						cnt++; 	
						sleep_ms(50); 																  /*wait 50ms*/
					}
				}
			break;
				
			default:		
				break;
		}
		if(req>=pCount)
		{	
			#ifdef PING_DEBUG
			printf("Ping Request = %d, PING_Reply = %d\r\n",req,rep);
			if(rep == req)
				printf( "PING SUCCESS\r\n " );
			else
				printf( "REPLY_ERROR\r\n " );	
			#endif  
		}
	}
}


uint8_t ping_request(uint8_t s, uint8_t *addr)
{
	uint16_t i;
	int32_t t;
	ping_reply_received = 0;
	PingRequest.Type = PING_REQUEST;                  				
	PingRequest.Code = CODE_ZERO;	                   					
	PingRequest.ID = htons(RandomID++);	       								
	PingRequest.SeqNum =htons(RandomSeqNum++);								
	for(i = 0 ; i < BUF_LEN; i++)
	{	             
		PingRequest.Data[i] = (i) % 8;		 				 						
	}
	PingRequest.CheckSum = 0;
	
	PingRequest.CheckSum = htons(checksum((uint8_t*)&PingRequest,sizeof(PingRequest))); 
	
	t = sendto(s,(uint8_t *)&PingRequest,sizeof(PingRequest),addr,3000);
	if(t==0)	
	{  
		printf( "\r\n Fail to send ping-reply packet  r\n") ;					
	}
	else
	{
		printf(" Ping %d.%d.%d.%d  \r\n",(addr[0]),  (addr[1]),  (addr[2]),  (addr[3]));
	}
	return 0;
} 


uint8_t ping_reply(uint8_t s, uint8_t *addr,  uint16_t rlen)
{
	uint16_t tmp_checksum;
	uint16_t len;
	uint16_t i;
	uint8_t data_buf[128];
	
	uint16_t port = 3000;
	PINGMSGR PingReply;
	len = recvfrom(s, (uint8_t *)data_buf,rlen,addr,&port);			
	if(data_buf[0] == PING_REPLY) 
	{
		PingReply.Type 		 = data_buf[0];
		PingReply.Code 		 = data_buf[1];
		PingReply.CheckSum   = (data_buf[3]<<8) + data_buf[2];
		PingReply.ID 		 = (data_buf[5]<<8) + data_buf[4];
		PingReply.SeqNum 	 = (data_buf[7]<<8) + data_buf[6];
		
		for(i=0; i<len-8 ; i++)
		{
			PingReply.Data[i] = data_buf[8+i];
		}
		tmp_checksum = ~checksum(data_buf,len);									
		if(tmp_checksum != 0xffff)
			printf("tmp_checksum = %x\r\n",tmp_checksum);
		else
		{
			printf(" Request from %d.%d.%d.%d  ID=%x SeqNum=%d \r\n",
			(addr[0]),  (addr[1]),  (addr[2]),  (addr[3]),htons(PingReply.ID),(rlen+6));			
			ping_reply_received =1;																
		}
	}
	else if(data_buf[0] == PING_REQUEST)
	{
		PingReply.Code 	 = data_buf[1];
		PingReply.Type 	 = data_buf[2];
		PingReply.CheckSum  = (data_buf[3]<<8) + data_buf[2];
		PingReply.ID 		 = (data_buf[5]<<8) + data_buf[4];
		PingReply.SeqNum 	 = (data_buf[7]<<8) + data_buf[6];		
		for(i=0; i<len-8 ; i++)
		{
			PingReply.Data[i] = data_buf[8+i];
		}
		tmp_checksum = PingReply.CheckSum;											
		PingReply.CheckSum = 0;
		if(tmp_checksum != PingReply.CheckSum)
		{
			printf( " \n CheckSum is in correct %x shold be %x \n",   (tmp_checksum),  htons(PingReply.CheckSum)) ;
		}
		else
		{		}
			printf("	Request from %d.%d.%d.%d  ID:%x SeqNum:%x  :data size %d bytes\r\n",
				(addr[0]),  (addr[1]),  (addr[2]),  (addr[3]),  (PingReply.ID),  (PingReply.SeqNum),  (rlen+6) );	   
			ping_reply_received =1;																
	}
	else
	{      
		printf(" Unkonwn msg. \n");
	}
	return 0;
}


